#include "GaitPlanner.h"
#include <string>
#include <cmath>
#include <iostream>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

GaitPlanner::GaitPlanner()
    : Gd_(Vector4d::Zero()),
      Go_(Vector4d::Zero()),
      t_fai_norm_(Vector4d::Zero()),
      s_fai_(Vector4d::Zero()),
      t_st_norm_(Vector4d::Zero()),
      t_sw_norm_(Vector4d::Zero()),

      p_touch_com_O_(Vector3d::Zero()),
      psi_touch_(0.0)
{
}

GaitPlanner::GaitPlanner::~GaitPlanner()
{
}

void GaitPlanner::setGait(double swingTime, double standTime, string mode)
{
    t_sw_ = swingTime;
    t_st_ = standTime;

    if (mode == "trot")
    {
        Gd_ = VectorXd::Constant(4, 0.5);
        Go_ << 0.5, 0, 0, 0.5;
    }
    // TODO:else if(mode == "walk")...

    setflag_ = 1;
}

void GaitPlanner::calculateGait(double currentTime)
{
    // 确保已设置输入
    if (setflag_ != 1)
    {
        cerr << "set gait first!";
        exit(1);
    }

    t_ = currentTime;

    T_gait_ = t_sw_ + t_st_;
    n_ = t_ / T_gait_ + 1; //!!
    t_ng_ = t_ - (n_ - 1) * T_gait_;
    t_ng_norm_ = t_ng_ / T_gait_;

    for (int i = 0; i < 4; i++)
    {
        if (t_ng_norm_ >= Go_[i])
        {
            t_fai_norm_[i] = t_ng_norm_ - Go_[i];
        }
        else
            t_fai_norm_[i] = t_ng_norm_ - Go_[i] + 1;
    }

    for (int i = 0; i < 4; i++)
    {
        if (t_fai_norm_[i] <= Gd_[i])
        {
            s_fai_[i] = 1;
            t_st_norm_[i] = t_fai_norm_[i] / Gd_[i];
            t_sw_norm_[i] = 0;
        }
        else
        {
            s_fai_[i] = 0;
            t_st_norm_[i] = 0;
            t_sw_norm_[i] = (t_fai_norm_[i] - Gd_[i]) / (1 - Gd_[i]);
        }
    }
}

Vector3d GaitPlanner::getp_hip_i_B_(int i) // i = 1,2,3,4
{
    Vector3d p_hip_i_B;
    int ksi = (i == 2 | i == 4) ? 1 : -1;
    int del = (i == 1 | i == 2) ? 1 : -1;

    p_hip_i_B << del * H_X + p_x_offset, ksi * (H_Y + L_1), 0;

    return p_hip_i_B;
}

void GaitPlanner::setState(Vector3d p_t_com_O, Vector3d v_t_com_O,
                           Matrix3d R_t_B_O, Vector3d omega_OB_O, double psi_t)
{
    p_t_com_O_ = p_t_com_O;
    v_t_com_O_ = v_t_com_O;
    R_t_B_O_ = R_t_B_O;
    omega_OB_O_ = omega_OB_O;
    psi_t_ = psi_t;
}

void GaitPlanner::setDesired(Vector3d v_t_com_d_B, double omega_t_z_d)
{
    v_t_com_d_B_ = v_t_com_d_B;
    omega_t_z_d_ = omega_t_z_d;
}

Vector3d GaitPlanner::getp_touch_hip_i_O(int i)
{
    // NOTE:i必须为摆动腿, i = 1,2,3,4
    Vector3d p_touch_hip_i_O;

    p_touch_com_O_ = p_t_com_O_ + R_t_B_O_ * v_t_com_d_B_ * (1 - t_sw_norm_[i - 1]) * t_sw_;
    psi_touch_ = psi_t_ + omega_t_z_d_ * (1 - t_sw_norm_[i - 1]) * t_sw_;

    p_touch_hip_i_O = p_touch_com_O_ + R("z", psi_touch_) * getp_hip_i_B_(i);
    return p_touch_hip_i_O;
}

Vector3d GaitPlanner::getp_swend_i_O(int i)
{
    // NOTE:i必须为摆动腿, i = 1,2,3,4
    Vector3d p_swend_i_O;
    // delp_1: R_t_B_O_ *v_t_com_d_B_ *t_st_ * 0.5;
    // delp_2: R("z", psi_touch_) * R("z", omega_t_z_d_ * t_st_ * 0.5) * getp_hip_i_B_(i) - getp_hip_i_B_(i);
    // delp_3: kp *(v_t_com_O_ - R_t_B_O_ * v_t_com_d_B_);
    // delp_4: p_t_com_O_(2) / 9.8 * v_t_com_O_.cross(omega_OB_O_);

    p_swend_i_O = (getp_touch_hip_i_O(i)) + (R_t_B_O_ * v_t_com_d_B_ * t_st_ * 0.5) + (R("z", psi_touch_) * R("z", omega_t_z_d_ * t_st_ * 0.5) * getp_hip_i_B_(i) - getp_hip_i_B_(i)) + (kp * (v_t_com_O_ - R_t_B_O_ * v_t_com_d_B_)) + (p_t_com_O_(2) / 9.8 * v_t_com_O_.cross(omega_OB_O_));
    return p_swend_i_O;
}
